#include "ProtocolParser.h"

ProtocolParser::ProtocolParser(QObject *parent) : QObject(parent) {}

QByteArray ProtocolParser::createPacket(uint8_t functionCode, const QByteArray &payload) {
    QByteArray packet;
    packet.append(0xAA);  // 帧头1
    packet.append(0x55);  // 帧头2
    packet.append(functionCode);  // 功能码

    uint16_t payloadLength = payload.size();
    packet.append(static_cast<char>(payloadLength & 0xFF));  // 长度低字节
    packet.append(static_cast<char>((payloadLength >> 8) & 0xFF));  // 长度高字节

    packet.append(payload);  // 负载数据

    // 计算CRC16并追加到包尾
    uint16_t crc = modbus_crc16(reinterpret_cast<const uint8_t *>(packet.constData()), packet.size());
    packet.append(static_cast<char>(crc & 0xFF));  // CRC低字节
    packet.append(static_cast<char>((crc >> 8) & 0xFF));  // CRC高字节

    return packet;
}

bool ProtocolParser::parsePacket(const QByteArray &data, uint8_t &functionCode, int &payloadOffset) {
    // 至少需要 7 字节：帧头2字节 + 功能码1字节 + 长度2字节 + CRC2字节
    if (data.size() < 7) {
        return false;
    }

    // 检查帧头
    if (static_cast<uint8_t>(data[0]) != 0xAA || static_cast<uint8_t>(data[1]) != 0x55) {
        return false;
    }

    // 读取功能码
    functionCode = static_cast<uint8_t>(data[2]);

    // 读取长度字段（低字节在前，高字节在后）
    uint16_t payloadLength = (static_cast<uint8_t>(data[4]) << 8) | static_cast<uint8_t>(data[3]);

    // 检查总长度是否正确
    if (data.size() != (7 + payloadLength)) {
        return false;
    }

    // 计算并校验CRC
    uint16_t computedCrc = modbus_crc16(reinterpret_cast<const uint8_t *>(data.constData()), data.size() - 2);
    uint16_t receivedCrc = (static_cast<uint8_t>(data[data.size() - 1]) << 8) | static_cast<uint8_t>(data[data.size() - 2]);

    if (computedCrc != receivedCrc) {
        return false;
    }

    // 数据包解析成功，返回 `payload` 偏移量
    payloadOffset = 5;  // `payload` 从第6个字节开始
    return true;
}

uint16_t ProtocolParser::modbus_crc16(const uint8_t *data, uint16_t length) {
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < length; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}
